/***************************************************************************
 创建者: 华磊
 开始时间: 2018.9.26
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************
 *  @file linematch.h
 *  功能: 直线探测功能
 *
 *                                                                         *
 ***************************************************************************/
#ifndef LINEMATCH_H
#define LINEMATCH_H
#include <qstring.h>
#ifdef D_USE_OPENCV
#include "opencv2/core/core.hpp"
#include "opencv2/nonfree/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/legacy/legacy.hpp"
#endif
#include <QMutex>
#include "globalDefine.h"

//using namespace cv;
using namespace std;

struct MathPoint
{
    double x;
    double y;
    double r;
};


class LineMatch
{
public:
    LineMatch();

public:
    int setLineFittingMethod(int methodIn);
    int setIsPrintVmarkProcessData(bool isPrint);
    int setLaserValidRegion(std::vector<double> region_x,std::vector<double> region_y);
    int setLaserIntensityFilter(double minumIntensityIn, double maxLaserRangeIn, double minAngleIn,
                                double maxAngleIn);
    int detectTwoLine(StructLineParameter lineParameter, const LaserDataInfo &laserData,
                      vector<double> &x_out, vector<double> &y_out, vector<double> &r_out,
                      int &result_line1, int &result_line2, double &gapDistanceOut, double &angleBiasOut,
                      double &line1_corelation, double &line2_corelation);

//    int showDebugWindow(int id,const Mat &imageIn);
    int setLineDetectPara(double rhoIn, double thetaIn, int thresholdIn,
                      double minLineLengthIn, double maxLineGapIn);
    int detectLine(QString filePath);
    /**
     * @brief detectVMark_mapFile
     * @param filePath
     * @param min_angle unit is rad
     * @param max_angle unit is rad
     * @param max_distance 交点离直线端点的最大距离 pix
     * @param allow_vDistance　多个vmark之间的距离 pix
     * @param allow_vAngle　多个vmark之间的角度 rad
     * @param x_out　ｖmark　的交点
     * @param y_out　ｖmark　的交点
     * @param r_out　ｖmark　的角度
     * @return
     */
    int detectVMark_mapFile(QString filePath, double min_angle, double max_angle, double max_distance, double allow_vDistance, double allow_vAngle,
                            vector<double> &x_out, vector<double> &y_out, vector<double> &r_out);
    /**
     * @brief detectVMark_mat
     * @param imageIn
     * @param min_angle unit is rad
     * @param max_angle unit is rad
     * @param max_distance 交点离直线端点的最大距离 pix
     * @param allow_vDistance　多个vmark之间的距离 pix
     * @param allow_vAngle　多个vmark之间的角度 rad
     * @param x_out　ｖmark　的交点 pix
     * @param y_out　ｖmark　的交点 pix
     * @param r_out　ｖmark　的角度 unit is rad
     * @return
     */
//    int detectVMark_mat(const Mat &imageIn, double min_angle, double max_angle, double max_distance, double allow_vDistance,
//                        double allow_vAngle, vector<double> &x_out, vector<double> &y_out, vector<double> &r_out,
//                        int &detectedLineCountOut, int &detectedVmartCountOut);

    bool isLaserPointOk_region(const MathPoint testPoint);
private:
    int processNoisePoint(double maxDistance, double a, double b, double c, QQueue<MathPoint> &pointList);
    int fittingLines(double standardCorelation,
                           QQueue<MathPoint>  tmpPointListForFit_line1, QQueue<MathPoint>  tmpPointListForFit_line2,
                           MathPoint &line1Start_out, MathPoint &line1End_out, MathPoint &line2Start_out, MathPoint &line2End_out, double &line1_corelation, double &line2_corelation);
    int detectReflectLine_old(double minLength,double maxLength,double allowMaxDeltaDistance,
                          const LaserDataInfo &laserData,vector<double> &x_out, vector<double> &y_out, vector<double> &r_out);
    int detectReflectLine_withFitting_old(int segmentJudgeMethod, double allowIntensityBias, double minLength, double maxLength, double allowMaxDeltaDistance,
                                      const LaserDataInfo &laserData, vector<MathPoint> &lineStart_out, vector<MathPoint> &lineEnd_out);
    int detectTwoReflectLine_withFitting(double minVAngle, double maxVAngle, double minLength, double maxLength,
                                       double allowMaxDeltaDistance, const LaserDataInfo &laserData,
                             vector<MathPoint> &line1ListStart_out, vector<MathPoint> &line1ListEnd_out,
                          vector<MathPoint> &line2ListStart_out, vector<MathPoint> &line2ListEnd_out, double &line1_corelation, double &line2_corelation, vector<double> &x_out, vector<double> &y_out, vector<double> &r_out, double &markAngleOut);
//    int normalNoisePoint(int samplePointNum, int maxNoisePointNum, const LaserDataInfo &laserDataIn, LaserDataInfo &laserDataOut);
    double calculateDistance(MathPoint point1,MathPoint point2);


    bool  myFitLine(QQueue<MathPoint> pointData,  MathPoint &point1Out, MathPoint &point2Out,
                    double &corationCoefficient, double &a_out, double &b_out, double &c_out);
    bool lineFit_liyuanbhu(const QQueue<MathPoint> &points, MathPoint &point1Out,MathPoint &point2Out
                                      ,double &corationCoefficient, double &a_out,double &b_out,double &c_out);
    bool lineFitLeastSquares(const QQueue<MathPoint> &points, MathPoint &point1Out, MathPoint &point2Out, double &corationCoefficient,
                             double &a_out, double &b_out, double &c_out);
    bool opencvFitLine(QQueue<MathPoint> pointData,  MathPoint &point1Out, MathPoint &point2Out,
                       double &corationCoefficient, double &a_out, double &b_out, double &c_out);
    bool isBreakPoint(const QQueue<MathPoint> miniSegmentQueueIn, double standardCoralation);
    bool getFitLineAngleBias(const QQueue<MathPoint> miniSegmentQueueIn, double allowCoration, double &angleOut);
    int detectLineAngleBias(bool isSkipPoint, double compareLength, double idealAngle_min, double idealAngle_max, double allowCoration,
                            const QQueue<MathPoint> miniSegmentQueueIn, QQueue<MathPoint> &firstSegmentQueueOut
                            , QQueue<MathPoint> &secondSegmentQueueOut, double &angleFinded, MathPoint &crossPointOut, double &markAngleOut);
//    bool getCrossPoint(Vec4i firstLine, Vec4i secondLine,Point2f &crossPoint,
//                                  double &crossDistance1,double &crossDistance2,double &angle_a, double &angle_b);
    bool getCrossPoint_double(vector<double> firstLine, vector<double> secondLine, MathPoint &crossPoint,
                                  double &crossDistance1, double &crossDistance2, double &angle_a, double &angle_b);
    bool getCrossPoint_abc(double a1,double b1, double c1, double a2,double b2, double c2, MathPoint &crossPoint);
//    int processPicture(const Mat &originPicture,Mat &transformedPicture);
#ifdef D_USE_OPENCV
    int detectLine(const Mat &imagineSource, vector<Vec4i> &linesOut);
    int detectVMark(const vector<Vec4i> &linesIn, double min_angle, double max_angle, double max_distance,
                    vector<double> &x_out, vector<double> &y_out, vector<double> &r_out);
#endif
    int detectStructLine(vector<double> p1_x, vector<double> p1_y, vector<double> p1_r,
                         vector<double> p2_x, vector<double> p2_y, vector<double> p2_r,
                          double min_gap, double max_gap, double minAngle, double maxAngle,
                    vector<double> &x_out, vector<double> &y_out, vector<double> &r_out,
                         vector<double> &gapDistanceOut, vector<double> &angleBiasOut);
    int detectStructLine_vmark(vector<MathPoint> line1Start, vector<MathPoint> line1End, vector<MathPoint> line2Start, vector<MathPoint> line2End,
                          double min_gap, double max_gap, double minAngle, double maxAngle,
                    vector<double> &x_out, vector<double> &y_out, vector<double> &r_out,
                         vector<double> &gapDistanceOut, vector<double> &angleBiasOut);
    int isStructLine(double p1_x, double p1_y, double p1_r, double p2_x, double p2_y, double p2_r,
                                double min_gap, double max_gap, double minAngle, double maxAngle,double &gapDistanceOut,double &angleBiasOut);
//    bool isVMark(Vec4i firstLine, Vec4i secondLine, double min_angle, double max_angle,
//                            double max_distance, double &x_out,double &y_out, double &r_out);
    bool calculateCrossPoint(MathPoint line1Start, MathPoint line1End, MathPoint line2Start, MathPoint line2End,
                            double &x_out, double &y_out, double &r_out);
    bool isVMark_structLine(MathPoint line1Start, MathPoint line1End, MathPoint line2Start, MathPoint line2End,
                            double min_gap, double max_gap, double minAngle, double maxAngle,
                            double &gapDistanceOut, double &angleBiasOut, double &x_out, double &y_out, double &r_out);
//    int displayVMark(const Mat &originPicture,vector<double> &x_out, vector<double> &y_out, vector<double> &r_out);

//    int displayLine(const Mat &originPicture,vector<Vec4i> &linesIn);

    int deleteTheSameVMark(double allowDistance,double allowAngle,
                           vector<double> &x_out, vector<double> &y_out, vector<double> &r_out);
    bool isTheSameVMark(double allowDistance,double allowAngle,double x_1, double y_1, double r_1,
                        double x_2, double y_2, double r_2);

    /**
     * @brief angleRotationJudgement 考虑哪种角度旋转方法最近
     * @param targetAngle
     * @param initialAngle
     * @return
     */
    double angleRotationJudgement(double targetAngle,double initialAngle);
    bool isLaserPointOk_laserFilter(const LaserDataInfo &laserData,int index);
    double getDistanceBetweenPointAndLine(double a,double b,double c,MathPoint point);
    bool isInRegion(double testx, double testy,std::vector<double> border_x, std::vector<double> border_y);

    bool isUseThisLaserPoint(const LaserDataInfo &laserData, int index, MathPoint &pointOut);
    bool isPointFittingVAngle(bool isSkipPoint, double compareLength, double idealAngle_min, double idealAngle_max, double allowCoration, const QQueue<MathPoint> miniSegmentQueueIn, double &line1_corelation, double &line2_corelation, double &angleFinded, MathPoint &crossPointOut, double &markAngleOut);

    int detectTwoReflectLine_withFitting_old(double minVAngle, double maxVAngle, double minLength, double maxLength,
                                                  double allowMaxDeltaDistance, const LaserDataInfo &laserData,
                                        vector<MathPoint> &line1ListStart_out, vector<MathPoint> &line1ListEnd_out,
                                     vector<MathPoint> &line2ListStart_out, vector<MathPoint> &line2ListEnd_out,
                                                  double &line1_corelation,double&line2_corelation);//作废
    int detectTwoLine_old(StructLineParameter lineParameter, const LaserDataInfo &laserData,
                      vector<double> &x_out, vector<double> &y_out, vector<double> &r_out,
                      int &result_line1, int &result_line2, double &gapDistanceOut, double &angleBiasOut,
                      double &line1_corelation, double &line2_corelation);//作废
private:
    double rho;
    double theta;
    int threshold_pt;
    double minLineLength;
    double maxLineGap;

    QMutex safeMutex;
//    std::vector<Vec4i> detectedLines;
    std::vector<double> x_detected;
    std::vector<double> y_detected;
    std::vector<double> r_detected;

    //激光过滤参数
    double minumIntensity;
    double maxLaserRange;
    double minAngle;
    double maxAngle;

    //激光有效区域
    std::vector<double> laserRegion_x;
    std::vector<double> laserRegion_y;

    bool isPrintVmarkProcessData;
    int lineFittingMethod;
    double laser_angleIncrement;//雷达角分辨率
};

#endif // LINEMATCH_H
